using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class MapWithLandmarks : Map
  {
    public enum LandmarkIDs {
      //Beacons
      centerLeftFlag,
      centerRightFlag,
      opponentLeftGoalPost,
      opponentRightGoalPost,
      ownLeftGoalPost,
      ownRightGoalPost,

      // 8 Ls
      opponentLeftL,
      opponentRightL,
      ownRightL,
      ownLeftL,
      opponentLeftPenaltyL,
      opponentRightPenaltyL,
      ownRightPenaltyL,
      ownLeftPenaltyL,
      // 12 Ls inside Ts
      ownLeftPenaltyTInnerL,
      ownLeftPenaltyTOuterL,
      ownRightPenaltyTInnerL,
      ownRightPenaltyTOuterL,

      centerLeftTOwnL,
      centerLeftTOpponentL,
      centerRightTOwnL,
      centerRightTOpponentL,

      opponentLeftPenaltyTInnerL,
      opponentLeftPenaltyTOuterL,
      opponentRightPenaltyTInnerL,
      opponentRightPenaltyTOuterL,

      numberOfLandmarkIDs
    };

    public enum LandMarkClasses
    {
      flag,
      corner,

      numberOfLandMarkClasses
    };

    public static LandMarkClasses GetLandmarkClass(LandmarkIDs landmarkID)
    {
      if (landmarkID < LandmarkIDs.ownRightGoalPost) return LandMarkClasses.flag;
      else return LandMarkClasses.corner;
    }

    private double[] xPositionOfLandmark;
    private double[] yPositionOfLandmark;

    public double[] XPositionOfLandmark
    {
      get { return xPositionOfLandmark; }
      set { xPositionOfLandmark = value; }
    }
    public double[] YPositionOfLandmark
    {
      get { return yPositionOfLandmark; }
      set { yPositionOfLandmark = value; }
    }

    public enum MapType
    {
      roboCupBeacons,
      lighthouses
    }

    public MapWithLandmarks()
    {
      InitWithRoboCupBeacons();
      InitFromFile();
    }
    
    public MapWithLandmarks(MapType mapType)
    {
      if (mapType == MapType.roboCupBeacons)
      {
        InitWithRoboCupBeacons();
        InitFromFile();
      }
      if(mapType == MapType.lighthouses) InitWithLighthouses();
    }

    private void InitWithRoboCupBeacons()
    {
      xPositionOfLandmark = new double[(int)LandmarkIDs.numberOfLandmarkIDs];
      yPositionOfLandmark = new double[(int)LandmarkIDs.numberOfLandmarkIDs];
      xPositionOfLandmark[(int)LandmarkIDs.centerLeftFlag] = 0;
      yPositionOfLandmark[(int)LandmarkIDs.centerLeftFlag] = 200;
      xPositionOfLandmark[(int)LandmarkIDs.centerRightFlag] = 0;
      yPositionOfLandmark[(int)LandmarkIDs.centerRightFlag] = -200;
      xPositionOfLandmark[(int)LandmarkIDs.opponentLeftGoalPost] = 300;
      yPositionOfLandmark[(int)LandmarkIDs.opponentLeftGoalPost] = 40;
      xPositionOfLandmark[(int)LandmarkIDs.opponentRightGoalPost] = 300;
      yPositionOfLandmark[(int)LandmarkIDs.opponentRightGoalPost] = -40;
      xPositionOfLandmark[(int)LandmarkIDs.ownLeftGoalPost] = -300;
      yPositionOfLandmark[(int)LandmarkIDs.ownLeftGoalPost] = 40;
      xPositionOfLandmark[(int)LandmarkIDs.ownRightGoalPost] = -300;
      yPositionOfLandmark[(int)LandmarkIDs.ownRightGoalPost] = -40;
    }

    private void InitFromFile()
    {
      //Field Dimensions
      Dictionary<string, double> fieldDimensions = new Dictionary<string, double>();
      List<string> dimensions = ConfigTool.GetSection(DrawingTools.GetPath() + "field.cfg", "dimensions");
      foreach (string entry in dimensions)
      {
        string[] parts = entry.Split(' ');
        string name = parts[0];
        string value = "";
        int counter = 1;
        while (counter < parts.Length)
        {
          if (parts[counter] != "") value = parts[counter];
          counter++;
        }
        if (IsANumber(value)) fieldDimensions[name] = double.Parse(value) / 10.0;
        else
        {
          if(value.StartsWith("-"))
          fieldDimensions[name] = -fieldDimensions[value.Replace("-","")];
        }
      }
      for (int i = 0; i < (int)LandmarkIDs.numberOfLandmarkIDs; i++)
      {
        string values = ConfigTool.GetSection(DrawingTools.GetPath() + "field.cfg", ((LandmarkIDs)i).ToString())[0];
        string[] parts = values.Split(' ');
        xPositionOfLandmark[i] = fieldDimensions[parts[0]];
        yPositionOfLandmark[i] = fieldDimensions[parts[1]];
      }
    }

    private static bool IsANumber(string value)
    {
      char[] valueAsCharArray = value.ToCharArray();
      bool valueIsANumber = true;
      for (int i = 0; i < valueAsCharArray.Length; i++) if (char.IsLetter(valueAsCharArray[i])) valueIsANumber = false;
      return valueIsANumber;
    }

    private void InitWithLighthouses()
    {
      xPositionOfLandmark = new double[2];
      yPositionOfLandmark = new double[2];
      xPositionOfLandmark[0] = 0;
      yPositionOfLandmark[0] = 200;
      xPositionOfLandmark[1] = 0;
      yPositionOfLandmark[1] = -200;
    }

    public override void Draw()
    {
      //coordinate system
      DebugDrawingTool.CreateLine("world:_map", new Vector2(-800, 0), new Vector2(800, 0), 3, Color.Gray);
      DebugDrawingTool.CreateLine("world:_map", new Vector2(0, -800), new Vector2(0, 800), 3, Color.Gray);

      //a colored circle for each landmark
      for (int i = 0; i < xPositionOfLandmark.Length; i++)
      {
        Color color = GetColorOfLandmark((LandmarkIDs)i);
        if( ((LandmarkIDs)i).ToString().EndsWith("L"))
        {
          DebugDrawingTool.CreateLine("world:_map",
            new Vector2(xPositionOfLandmark[i] - 5, yPositionOfLandmark[i]),
            new Vector2(xPositionOfLandmark[i] + 5, yPositionOfLandmark[i]), 1, color);
          DebugDrawingTool.CreateLine("world:_map",
            new Vector2(xPositionOfLandmark[i], yPositionOfLandmark[i] - 5),
            new Vector2(xPositionOfLandmark[i], yPositionOfLandmark[i] + 5), 1, color);
        }
        else
        {
          DebugDrawingTool.CreateFilledCircle("world:_map", new Vector2(xPositionOfLandmark[i], yPositionOfLandmark[i]), 10, color);
          DebugDrawingTool.CreateLine("world:_map", 
            new Vector2(xPositionOfLandmark[i] - 3, yPositionOfLandmark[i]),
            new Vector2(xPositionOfLandmark[i] + 3, yPositionOfLandmark[i]), 1, DrawingTools.GetShadedColor(0.5, ref color));
          DebugDrawingTool.CreateLine("world:_map", 
            new Vector2(xPositionOfLandmark[i], yPositionOfLandmark[i] - 3),
            new Vector2(xPositionOfLandmark[i], yPositionOfLandmark[i] + 3), 1, DrawingTools.GetShadedColor(0.5, ref color));
        }
      }
    }

    public static Color GetColorOfLandmark(LandmarkIDs landmarkID)
    {
      Color color = Color.Black;
      switch (landmarkID)
      {
        case LandmarkIDs.centerLeftFlag: color = Color.Green; break;
        case LandmarkIDs.centerRightFlag: color = Color.Red; break;
        case LandmarkIDs.opponentLeftGoalPost: color = Color.Blue; break;
        case LandmarkIDs.opponentRightGoalPost: color = Color.Orange; break;
        case LandmarkIDs.ownLeftGoalPost: color = Color.Yellow; break;
        case LandmarkIDs.ownRightGoalPost: color = Color.Brown; break;
        default: color = Color.Black; break;
      }
      return color;
    }
  }
}
